/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.model;

import com.team1160.breakaway.api.Constants;

/**
 *
 * @author Team 1160 Firebird Robotics (saketh kasibatla)
 */
public class RobotStateReader {

    //final instance fields
    ////////////////////////////////////////////////////////////////////////////
    protected static RobotStateReader DEFAULT_INSTANCE;
    protected final Model model;
    
    protected final double DELTA = 0.05; //TODO Mango check my delta and fine tune it


    //Constructor
    ////////////////////////////////////////////////////////////////////////////
    private RobotStateReader(Model model)
    {
        this.model=model;
    }

    //accessor methods
    ////////////////////////////////////////////////////////////////////////////
    
    public static RobotStateReader getInstance()
    {
        if (DEFAULT_INSTANCE == null)
        {
            DEFAULT_INSTANCE = new RobotStateReader(Model.getInstance());
        }
        return DEFAULT_INSTANCE;
    }

    //---------------------------------------------------------------------
    
    public RobotState getRobotState()
    {
        //new robot state
        RobotState state = new RobotState();

        //-------------------------

        //dribbler and kicker
        state.setDribblerRunning(model.getDribblerMotorValue() != 0.0);
        state.setKickerKicking(model.getSolenoidValue(Constants.ID_KICKER_SOLENOID_FIRE_BACK) &&
                                !model.getSolenoidValue(Constants.ID_KICKER_SOLENOID_FIRE_FRONT));
        state.setLatched(!model.getSolenoidValue(Constants.ID_KICKER_SOLENOID_LATCH_BACK) &&
                                model.getSolenoidValue(Constants.ID_KICKER_SOLENOID_LATCH_FRONT));
        
        //-------------------------

        //drive motors

        state.setDriveTrainMotorValue(Constants.ID_DRIVETRAIN_LEFT_BACK,
                                      model.getDriveTrainMotorValue(Constants.ID_DRIVETRAIN_LEFT_BACK));
        state.setDriveTrainMotorValue(Constants.ID_DRIVETRAIN_LEFT_FRONT,
                                      model.getDriveTrainMotorValue(Constants.ID_DRIVETRAIN_LEFT_FRONT));
        state.setDriveTrainMotorValue(Constants.ID_DRIVETRAIN_RIGHT_BACK,
                                      model.getDriveTrainMotorValue(Constants.ID_DRIVETRAIN_RIGHT_BACK));
        state.setDriveTrainMotorValue(Constants.ID_DRIVETRAIN_RIGHT_FRONT,
                                      model.getDriveTrainMotorValue(Constants.ID_DRIVETRAIN_RIGHT_FRONT));

        //-------------------------

        //gyro and accelerometer

        state.setGyroAngle(model.getGyroAngle());
        state.setAccelRate(model.getAccelRate());

        //-------------------------

        //target info
        
        state.setTarget(model.getTargetCenter());
        state.setNormalizedTarget(model.getNormalizedTargetCenter());

        
        //-------------------------

        return state;
    }

    //---------------------------------------------------------------------

    public boolean withinDelta(double val, double reference){
        if((val>(reference-this.DELTA))&&(val<(reference+this.DELTA))){
            return true;
        }
        return false;
    }

    //---------------------------------------------------------------------
    
}
